{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": 11,
   "metadata": {
    "pycharm": {
     "is_executing": false
    }
   },
   "outputs": [],
   "source": [
    "import copy\n",
    "import math\n",
    "import itertools\n",
    "import numpy as np \n",
    "import matplotlib.pyplot as plt\n",
    "from graph_based_slam import calc_rotational_matrix, calc_jacobian, cal_observation_sigma, \\\n",
    "                             calc_input, observation, motion_model, Edge, pi_2_pi\n",
    "\n",
    "%matplotlib inline\n",
    "np.set_printoptions(precision=3, suppress=True)\n",
    "np.random.seed(0)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Introduction\n",
    "\n",
    "In contrast to the probabilistic approaches for solving SLAM, such as EKF, UKF, particle filters, and so on, the graph technique formulates the SLAM as an optimization problem.  It is mostly used to solve the full SLAM problem in an offline fashion, i.e. optimize all the poses of the robot after the path has been traversed. However, some variants are availble that uses graph-based approaches to perform online estimation or to solve for a subset of the poses.\n",
    "\n",
    "GraphSLAM uses the motion information as well as the observations of the environment to create least square problem that can be solved using standard optimization techniques.\n",
    "\n",
    "### Minimal Example\n",
    "The following example illustrates the main idea behind graphSLAM. \n",
    "A simple case of a 1D robot is considered that can only move in 1 direction. The robot is commanded to move forward with a control input $u_t=1$, however, the motion is not perfect and the measured odometry will deviate from the true path. At each timestep the robot can observe its environment, for this simple case as well, there is only a single landmark at coordinates $x=3$. The measured observations are the range between the robot and landmark. These measurements are also subjected to noise. No bearing information is required since this is a 1D problem.\n",
    "\n",
    "To solve this, graphSLAM creates what is called as the system information matrix $\\Omega$ also referred to as $H$ and the information vector $\\xi$ also known as $b$. The entries are created based on the information of the motion and the observation."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 12,
   "metadata": {
    "pycharm": {
     "is_executing": false
    }
   },
   "outputs": [
    {
     "data": {
      "text/plain": "<Figure size 432x288 with 1 Axes>",
      "image/png": "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\n"
     },
     "metadata": {
      "needs_background": "light"
     },
     "output_type": "display_data"
    },
    {
     "name": "stdout",
     "text": [
      "The determinant of H:  0.0\n",
      "The determinant of H after anchoring constraint:  18.750000000000007\n",
      "Running graphSLAM ...\n",
      "Odometry values after optimzation: \n",
      " [[-0. ]\n",
      " [ 0.9]\n",
      " [ 1.9]]\n"
     ],
     "output_type": "stream"
    },
    {
     "data": {
      "text/plain": "<Figure size 432x288 with 1 Axes>",
      "image/png": "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\n"
     },
     "metadata": {
      "needs_background": "light"
     },
     "output_type": "display_data"
    }
   ],
   "source": [
    "R = 0.2\n",
    "Q = 0.2\n",
    "N = 3\n",
    "graphics_radius = 0.1\n",
    "\n",
    "odom = np.empty((N,1))\n",
    "obs = np.empty((N,1))\n",
    "x_true = np.empty((N,1))\n",
    "\n",
    "landmark = 3\n",
    "# Simulated readings of odometry and observations\n",
    "x_true[0], odom[0], obs[0] = 0.0, 0.0, 2.9\n",
    "x_true[1], odom[1], obs[1] = 1.0, 1.5, 2.0\n",
    "x_true[2], odom[2], obs[2] = 2.0, 2.4, 1.0\n",
    "\n",
    "hxDR = copy.deepcopy(odom)\n",
    "# Visualization\n",
    "plt.plot(landmark,0, '*k', markersize=30)\n",
    "for i in range(N):\n",
    "    plt.plot(odom[i], 0, '.', markersize=50, alpha=0.8, color='steelblue')\n",
    "    plt.plot([odom[i], odom[i] + graphics_radius],\n",
    "             [0,0], 'r')\n",
    "    plt.text(odom[i], 0.02, \"X_{}\".format(i), fontsize=12)\n",
    "    plt.plot(obs[i]+odom[i],0,'.', markersize=25, color='brown')\n",
    "    plt.plot(x_true[i],0,'.g', markersize=20)\n",
    "plt.grid()    \n",
    "plt.show()\n",
    "\n",
    "\n",
    "# Defined as a function to facilitate iteration\n",
    "def get_H_b(odom, obs):\n",
    "    \"\"\"\n",
    "    Create the information matrix and information vector. This implementation is \n",
    "    based on the concept of virtual measurement i.e. the observations of the\n",
    "    landmarks are converted into constraints (edges) between the nodes that\n",
    "    have observed this landmark.\n",
    "    \"\"\"\n",
    "    measure_constraints = {}\n",
    "    omegas = {}\n",
    "    zids = list(itertools.combinations(range(N),2))\n",
    "    H = np.zeros((N,N))\n",
    "    b = np.zeros((N,1))\n",
    "    for (t1, t2) in zids:\n",
    "        x1 = odom[t1]\n",
    "        x2 = odom[t2]\n",
    "        z1 = obs[t1]\n",
    "        z2 = obs[t2]\n",
    "        \n",
    "        # Adding virtual measurement constraint\n",
    "        measure_constraints[(t1,t2)] = (x2-x1-z1+z2)\n",
    "        omegas[(t1,t2)] = (1 / (2*Q))\n",
    "        \n",
    "        # populate system's information matrix and vector\n",
    "        H[t1,t1] += omegas[(t1,t2)]\n",
    "        H[t2,t2] += omegas[(t1,t2)]\n",
    "        H[t2,t1] -= omegas[(t1,t2)]\n",
    "        H[t1,t2] -= omegas[(t1,t2)]\n",
    "\n",
    "        b[t1] += omegas[(t1,t2)] * measure_constraints[(t1,t2)]\n",
    "        b[t2] -= omegas[(t1,t2)] * measure_constraints[(t1,t2)]\n",
    "\n",
    "    return H, b\n",
    "\n",
    "\n",
    "H, b = get_H_b(odom, obs)\n",
    "print(\"The determinant of H: \", np.linalg.det(H))\n",
    "H[0,0] += 1 # np.inf ?\n",
    "print(\"The determinant of H after anchoring constraint: \", np.linalg.det(H))\n",
    "\n",
    "for i in range(5):\n",
    "    H, b = get_H_b(odom, obs)\n",
    "    H[(0,0)] += 1\n",
    "    \n",
    "    # Recover the posterior over the path\n",
    "    dx = np.linalg.inv(H) @ b\n",
    "    odom += dx\n",
    "    # repeat till convergence\n",
    "print(\"Running graphSLAM ...\")    \n",
    "print(\"Odometry values after optimzation: \\n\", odom)\n",
    "\n",
    "plt.figure()\n",
    "plt.plot(x_true, np.zeros(x_true.shape), '.', markersize=20, label='Ground truth')\n",
    "plt.plot(odom, np.zeros(x_true.shape), '.', markersize=20, label='Estimation')\n",
    "plt.plot(hxDR, np.zeros(x_true.shape), '.', markersize=20, label='Odom')\n",
    "plt.legend()\n",
    "plt.grid()\n",
    "plt.show()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "In particular, the tasks are split into 2 parts, graph construction, and graph optimization. \n",
    "### Graph Construction\n",
    "\n",
    "Firstly the nodes are defined $\\boldsymbol{x} = \\boldsymbol{x}_{1:n}$  such that each node is the pose of the robot at time $t_i$\n",
    "Secondly, the edges i.e. the constraints, are constructed according to the following conditions:\n",
    "\n",
    "* robot moves from $\\boldsymbol{x}_i$ to $\\boldsymbol{x}_j$. This edge corresponds to the odometry measurement. Relative motion constraints (Not included in the previous minimal example).\n",
    "* Measurement constraints, this can be done in two ways:\n",
    "    * The information matrix is set in such a way that it includes the landmarks in the map as well. Then the constraints can be entered in a straightforward fashion between a node $\\boldsymbol{x}_i$ and some landmark $m_k$\n",
    "    * Through creating a virtual measurement among all the node that have observed the same landmark. More concretely, robot observes the same landmark from $\\boldsymbol{x}_i$ and $\\boldsymbol{x}_j$. Relative measurement constraint. The \"virtual measurement\" $\\boldsymbol{z}_{ij}$, which is the estimated pose of $\\boldsymbol{x}_j$ as seen from the node $\\boldsymbol{x}_i$. The virtual measurement can then be entered in the information matrix and vector in a similar fashion to the motion constraints.\n",
    "\n",
    "An edge is fully characterized by the values of the error (entry to information vector) and the local information matrix (entry to the system's information vector). The larger the local information matrix (lower $Q$ or $R$) the values that this edge will contribute with.\n",
    "\n",
    "Important Notes:\n",
    "\n",
    "* The addition to the information matrix and vector are added to the earlier values.\n",
    "* In the case of 2D robot, the constraints will be non-linear, therefore, a Jacobian of the error w.r.t the states is needed when updated $H$ and $b$.\n",
    "* The anchoring constraint is needed in order to avoid having a singular information matri.\n",
    "\n",
    "\n",
    "### Graph Optimization\n",
    "\n",
    "The result from this formulation yields an overdetermined system of equations.\n",
    "The goal after constructing the graph is to find: $x^* = \\underset{x}{\\mathrm{argmin}} ~ \\underset{ij} \\Sigma ~ f(e_{ij}) $, where $f$ is some error function that depends on the edges between to related nodes $i$ and $j$. The derivation in the references arrive at the solution for $x^* = H^{-1}b$ \n",
    "\n",
    "\n",
    "### Planar Example:\n",
    "\n",
    "Now we will go through an example with a more realistic case of a 2D robot with 3DoF, namely, $[x, y, \\theta]^T$"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 13,
   "metadata": {
    "pycharm": {
     "is_executing": false
    }
   },
   "outputs": [
    {
     "data": {
      "text/plain": "<Figure size 432x288 with 1 Axes>",
      "image/png": "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\n"
     },
     "metadata": {
      "needs_background": "light"
     },
     "output_type": "display_data"
    }
   ],
   "source": [
    "#  Simulation parameter\n",
    "Qsim = np.diag([0.01, np.deg2rad(0.010)])**2 # error added to range and bearing\n",
    "Rsim = np.diag([0.1, np.deg2rad(1.0)])**2 # error added to [v, w]\n",
    "\n",
    "DT = 2.0  # time tick [s]\n",
    "SIM_TIME = 100.0  # simulation time [s]\n",
    "MAX_RANGE = 30.0  # maximum observation range\n",
    "STATE_SIZE = 3  # State size [x,y,yaw]\n",
    "\n",
    "# TODO: Why not use Qsim ? \n",
    "# Covariance parameter of Graph Based SLAM\n",
    "C_SIGMA1 = 0.1\n",
    "C_SIGMA2 = 0.1\n",
    "C_SIGMA3 = np.deg2rad(1.0)\n",
    "\n",
    "MAX_ITR = 20  # Maximum iteration during optimization\n",
    "timesteps = 1\n",
    "\n",
    "# consider only 2 landmarks for simplicity\n",
    "# RFID positions [x, y, yaw]\n",
    "RFID = np.array([[10.0, -2.0, 0.0],\n",
    "#                  [15.0, 10.0, 0.0],\n",
    "#                  [3.0, 15.0, 0.0],\n",
    "#                  [-5.0, 20.0, 0.0],\n",
    "#                  [-5.0, 5.0, 0.0]\n",
    "                 ])\n",
    "\n",
    "# State Vector [x y yaw v]'\n",
    "xTrue = np.zeros((STATE_SIZE, 1))\n",
    "xDR = np.zeros((STATE_SIZE, 1))  # Dead reckoning\n",
    "xTrue[2] = np.deg2rad(45)\n",
    "xDR[2] = np.deg2rad(45)\n",
    "# history initial values\n",
    "hxTrue = xTrue\n",
    "hxDR = xTrue\n",
    "_, z, _, _ = observation(xTrue, xDR, np.array([[0,0]]).T, RFID)\n",
    "hz = [z]\n",
    "\n",
    "for i in range(timesteps):\n",
    "    u = calc_input()\n",
    "    xTrue, z, xDR, ud = observation(xTrue, xDR, u, RFID)\n",
    "    hxDR = np.hstack((hxDR, xDR))\n",
    "    hxTrue = np.hstack((hxTrue, xTrue))\n",
    "    hz.append(z)\n",
    "\n",
    "# visualize\n",
    "graphics_radius = 0.3\n",
    "plt.plot(RFID[:, 0], RFID[:, 1], \"*k\", markersize=20)\n",
    "plt.plot(hxDR[0, :], hxDR[1, :], '.', markersize=50, alpha=0.8, label='Odom')\n",
    "plt.plot(hxTrue[0, :], hxTrue[1, :], '.', markersize=20, alpha=0.6, label='X_true')\n",
    "\n",
    "for i in range(hxDR.shape[1]):\n",
    "    x = hxDR[0, i]\n",
    "    y = hxDR[1, i]\n",
    "    yaw = hxDR[2, i]\n",
    "    plt.plot([x, x + graphics_radius * np.cos(yaw)],\n",
    "             [y, y + graphics_radius * np.sin(yaw)], 'r')\n",
    "    d = hz[i][:, 0]\n",
    "    angle = hz[i][:, 1]\n",
    "    plt.plot([x + d * np.cos(angle + yaw)], [y + d * np.sin(angle + yaw)], '.',\n",
    "             markersize=20, alpha=0.7)\n",
    "    plt.legend()\n",
    "plt.grid()    \n",
    "plt.show()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 14,
   "metadata": {
    "pycharm": {
     "is_executing": false
    }
   },
   "outputs": [],
   "source": [
    "# Copy the data to have a consistent naming with the .py file\n",
    "zlist = copy.deepcopy(hz)\n",
    "x_opt = copy.deepcopy(hxDR)\n",
    "xlist = copy.deepcopy(hxDR)\n",
    "number_of_nodes = x_opt.shape[1]\n",
    "n = number_of_nodes * STATE_SIZE"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "After the data has been saved, the graph will be constructed by looking at each pair for nodes. The virtual measurement is only created if two nodes have observed the same landmark at different points in time. The next cells are a walk through for a single iteration of graph construction -> optimization -> estimate update. "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 15,
   "metadata": {
    "pycharm": {
     "is_executing": false
    }
   },
   "outputs": [
    {
     "name": "stdout",
     "text": [
      "Node combinations: \n",
      " [(0, 1)]\n",
      "Node 0 observed landmark with ID 0.0\n",
      "Node 1 observed landmark with ID 0.0\n"
     ],
     "output_type": "stream"
    }
   ],
   "source": [
    "# get all the possible combination of the different node\n",
    "zids = list(itertools.combinations(range(len(zlist)), 2))\n",
    "print(\"Node combinations: \\n\", zids)\n",
    "\n",
    "for i in range(xlist.shape[1]):\n",
    "    print(\"Node {} observed landmark with ID {}\".format(i, zlist[i][0, 3]))"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "In the following code snippet the error based on the virtual measurement between node 0 and 1 will be created.\n",
    "The equations for the error is as follows: \n",
    "$e_{ij}^x = x_j + d_j cos(\\psi_j + \\theta_j) - x_i - d_i cos(\\psi_i + \\theta_i) $\n",
    "\n",
    "$e_{ij}^y = y_j + d_j sin(\\psi_j + \\theta_j) - y_i - d_i sin(\\psi_i + \\theta_i) $\n",
    "\n",
    "$e_{ij}^x = \\psi_j + \\theta_j - \\psi_i - \\theta_i $\n",
    "\n",
    "Where $[x_i, y_i, \\psi_i]$ is the pose for node $i$ and similarly for node $j$, $d$ is the measured distance at nodes $i$ and $j$, and $\\theta$ is the measured bearing to the landmark. The difference is visualized with the figure in the next cell.\n",
    "\n",
    "In case of perfect motion and perfect measurement the error shall be zero since $ x_j + d_j cos(\\psi_j + \\theta_j)$ should equal $x_i + d_i cos(\\psi_i + \\theta_i)$"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 16,
   "metadata": {
    "pycharm": {
     "is_executing": false
    }
   },
   "outputs": [
    {
     "name": "stdout",
     "text": [
      "0.0 1.427649841628278 -2.0126109674819155 -3.524048014922737\n",
      "For nodes (0, 1)\n",
      "Added edge with errors: \n",
      " [[-0.02 ]\n",
      " [-0.084]\n",
      " [ 0.   ]]\n"
     ],
     "output_type": "stream"
    },
    {
     "data": {
      "text/plain": "<Figure size 432x288 with 1 Axes>",
      "image/png": "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\n"
     },
     "metadata": {
      "needs_background": "light"
     },
     "output_type": "display_data"
    }
   ],
   "source": [
    "# Initialize edges list\n",
    "edges = []\n",
    "\n",
    "# Go through all the different combinations\n",
    "for (t1, t2) in zids:\n",
    "    x1, y1, yaw1 = xlist[0, t1], xlist[1, t1], xlist[2, t1]\n",
    "    x2, y2, yaw2 = xlist[0, t2], xlist[1, t2], xlist[2, t2]\n",
    "    \n",
    "    # All nodes have valid observation with ID=0, therefore, no data association condition\n",
    "    iz1 = 0\n",
    "    iz2 = 0\n",
    "    \n",
    "    d1 = zlist[t1][iz1, 0]\n",
    "    angle1, phi1 = zlist[t1][iz1, 1], zlist[t1][iz1, 2]\n",
    "    d2 = zlist[t2][iz2, 0]\n",
    "    angle2, phi2 = zlist[t2][iz2, 1], zlist[t2][iz2, 2]\n",
    "    \n",
    "    # find angle between observation and horizontal \n",
    "    tangle1 = pi_2_pi(yaw1 + angle1)\n",
    "    tangle2 = pi_2_pi(yaw2 + angle2)\n",
    "    \n",
    "    # project the observations \n",
    "    tmp1 = d1 * math.cos(tangle1)\n",
    "    tmp2 = d2 * math.cos(tangle2)\n",
    "    tmp3 = d1 * math.sin(tangle1)\n",
    "    tmp4 = d2 * math.sin(tangle2)\n",
    "    \n",
    "    edge = Edge()\n",
    "    print(y1,y2, tmp3, tmp4)\n",
    "    # calculate the error of the virtual measurement\n",
    "    # node 1 as seen from node 2 throught the observations 1,2\n",
    "    edge.e[0, 0] = x2 - x1 - tmp1 + tmp2\n",
    "    edge.e[1, 0] = y2 - y1 - tmp3 + tmp4\n",
    "    edge.e[2, 0] = pi_2_pi(yaw2 - yaw1 - tangle1 + tangle2)\n",
    "\n",
    "    edge.d1, edge.d2 = d1, d2\n",
    "    edge.yaw1, edge.yaw2 = yaw1, yaw2\n",
    "    edge.angle1, edge.angle2 = angle1, angle2\n",
    "    edge.id1, edge.id2 = t1, t2\n",
    "    \n",
    "    edges.append(edge)\n",
    "    \n",
    "    print(\"For nodes\",(t1,t2))\n",
    "    print(\"Added edge with errors: \\n\", edge.e)\n",
    "    \n",
    "    # Visualize measurement projections\n",
    "    plt.plot(RFID[0, 0], RFID[0, 1], \"*k\", markersize=20)\n",
    "    plt.plot([x1,x2],[y1,y2], '.', markersize=50, alpha=0.8)\n",
    "    plt.plot([x1, x1 + graphics_radius * np.cos(yaw1)],\n",
    "             [y1, y1 + graphics_radius * np.sin(yaw1)], 'r')\n",
    "    plt.plot([x2, x2 + graphics_radius * np.cos(yaw2)],\n",
    "             [y2, y2 + graphics_radius * np.sin(yaw2)], 'r')\n",
    "    \n",
    "    plt.plot([x1,x1+tmp1], [y1,y1], label=\"obs 1 x\")\n",
    "    plt.plot([x2,x2+tmp2], [y2,y2], label=\"obs 2 x\")\n",
    "    plt.plot([x1,x1], [y1,y1+tmp3], label=\"obs 1 y\")\n",
    "    plt.plot([x2,x2], [y2,y2+tmp4], label=\"obs 2 y\")\n",
    "    plt.plot(x1+tmp1, y1+tmp3, 'o')\n",
    "    plt.plot(x2+tmp2, y2+tmp4, 'o')\n",
    "plt.legend()\n",
    "plt.grid()\n",
    "plt.show()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Since the constraints equations derived before are non-linear, linearization is needed before we can insert them into the information matrix and information vector. Two jacobians \n",
    "\n",
    "$A = \\frac{\\partial e_{ij}}{\\partial \\boldsymbol{x}_i}$ as $\\boldsymbol{x}_i$ holds the three variabls x, y, and theta.\n",
    "Similarly, \n",
    "$B = \\frac{\\partial e_{ij}}{\\partial \\boldsymbol{x}_j}$ "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 17,
   "metadata": {
    "pycharm": {
     "is_executing": false
    }
   },
   "outputs": [
    {
     "name": "stdout",
     "text": [
      "The determinant of H:  0.0\n",
      "The determinant of H after origin constraint:  716.1972439134893\n"
     ],
     "output_type": "stream"
    },
    {
     "data": {
      "text/plain": "<Figure size 432x288 with 2 Axes>",
      "image/png": "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\n"
     },
     "metadata": {
      "needs_background": "light"
     },
     "output_type": "display_data"
    },
    {
     "data": {
      "text/plain": "<Figure size 432x288 with 2 Axes>",
      "image/png": "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\n"
     },
     "metadata": {
      "needs_background": "light"
     },
     "output_type": "display_data"
    }
   ],
   "source": [
    "# Initialize the system information matrix and information vector\n",
    "H = np.zeros((n, n))\n",
    "b = np.zeros((n, 1))\n",
    "x_opt = copy.deepcopy(hxDR)\n",
    "\n",
    "for edge in edges:\n",
    "    id1 = edge.id1 * STATE_SIZE\n",
    "    id2 = edge.id2 * STATE_SIZE\n",
    "    \n",
    "    t1 = edge.yaw1 + edge.angle1\n",
    "    A = np.array([[-1.0, 0, edge.d1 * math.sin(t1)],\n",
    "                  [0, -1.0, -edge.d1 * math.cos(t1)],\n",
    "                  [0, 0, -1.0]])\n",
    "\n",
    "    t2 = edge.yaw2 + edge.angle2\n",
    "    B = np.array([[1.0, 0, -edge.d2 * math.sin(t2)],\n",
    "                  [0, 1.0, edge.d2 * math.cos(t2)],\n",
    "                  [0, 0, 1.0]])\n",
    "    \n",
    "    # TODO: use Qsim instead of sigma\n",
    "    sigma = np.diag([C_SIGMA1, C_SIGMA2, C_SIGMA3])\n",
    "    Rt1 = calc_rotational_matrix(tangle1)\n",
    "    Rt2 = calc_rotational_matrix(tangle2)\n",
    "    edge.omega = np.linalg.inv(Rt1 @ sigma @ Rt1.T + Rt2 @ sigma @ Rt2.T)\n",
    "\n",
    "    # Fill in entries in H and b\n",
    "    H[id1:id1 + STATE_SIZE, id1:id1 + STATE_SIZE] += A.T @ edge.omega @ A\n",
    "    H[id1:id1 + STATE_SIZE, id2:id2 + STATE_SIZE] += A.T @ edge.omega @ B\n",
    "    H[id2:id2 + STATE_SIZE, id1:id1 + STATE_SIZE] += B.T @ edge.omega @ A\n",
    "    H[id2:id2 + STATE_SIZE, id2:id2 + STATE_SIZE] += B.T @ edge.omega @ B\n",
    "\n",
    "    b[id1:id1 + STATE_SIZE] += (A.T @ edge.omega @ edge.e)\n",
    "    b[id2:id2 + STATE_SIZE] += (B.T @ edge.omega @ edge.e)\n",
    "   \n",
    "\n",
    "print(\"The determinant of H: \", np.linalg.det(H))\n",
    "plt.figure()\n",
    "plt.subplot(1,2,1)\n",
    "plt.imshow(H, extent=[0, n, 0, n])\n",
    "plt.subplot(1,2,2)\n",
    "plt.imshow(b, extent=[0, 1, 0, n])\n",
    "plt.show()    \n",
    "\n",
    "# Fix the origin, multiply by large number gives same results but better visualization\n",
    "H[0:STATE_SIZE, 0:STATE_SIZE] += np.identity(STATE_SIZE)\n",
    "print(\"The determinant of H after origin constraint: \", np.linalg.det(H))    \n",
    "plt.figure()\n",
    "plt.subplot(1,2,1)\n",
    "plt.imshow(H, extent=[0, n, 0, n])\n",
    "plt.subplot(1,2,2)\n",
    "plt.imshow(b, extent=[0, 1, 0, n])\n",
    "plt.show()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 18,
   "metadata": {
    "pycharm": {
     "is_executing": false
    }
   },
   "outputs": [
    {
     "name": "stdout",
     "text": [
      "dx: \n",
      " [[-0.   ]\n",
      " [-0.   ]\n",
      " [ 0.   ]\n",
      " [ 0.02 ]\n",
      " [ 0.084]\n",
      " [-0.   ]]\n",
      "ground truth: \n",
      "  [[0.    1.414]\n",
      " [0.    1.414]\n",
      " [0.785 0.985]]\n",
      "Odom: \n",
      " [[0.    1.428]\n",
      " [0.    1.428]\n",
      " [0.785 0.976]]\n",
      "SLAM: \n",
      " [[-0.     1.448]\n",
      " [-0.     1.512]\n",
      " [ 0.785  0.976]]\n",
      "\n",
      "graphSLAM localization error:  0.010729072751057656\n",
      "Odom localization error:  0.0004460978857535104\n"
     ],
     "output_type": "stream"
    }
   ],
   "source": [
    "# Find the solution (first iteration)\n",
    "dx = - np.linalg.inv(H) @ b\n",
    "for i in range(number_of_nodes):\n",
    "    x_opt[0:3, i] += dx[i * 3:i * 3 + 3, 0]\n",
    "print(\"dx: \\n\",dx)\n",
    "print(\"ground truth: \\n \",hxTrue)\n",
    "print(\"Odom: \\n\", hxDR)\n",
    "print(\"SLAM: \\n\", x_opt)\n",
    "\n",
    "# performance will improve with more iterations, nodes and landmarks.\n",
    "print(\"\\ngraphSLAM localization error: \", np.sum((x_opt - hxTrue) ** 2))\n",
    "print(\"Odom localization error: \", np.sum((hxDR - hxTrue) ** 2))"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### The references:\n",
    "\n",
    "\n",
    "* http://robots.stanford.edu/papers/thrun.graphslam.pdf\n",
    "\n",
    "* http://ais.informatik.uni-freiburg.de/teaching/ss13/robotics/slides/16-graph-slam.pdf\n",
    "\n",
    "* http://www2.informatik.uni-freiburg.de/~stachnis/pdf/grisetti10titsmag.pdf\n",
    "\n",
    "N.B.\n",
    "An additional step is required that uses the estimated path to update the belief regarding the map."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 18,
   "metadata": {
    "pycharm": {
     "is_executing": false
    }
   },
   "outputs": [],
   "source": []
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.6.5"
  },
  "pycharm": {
   "stem_cell": {
    "cell_type": "raw",
    "source": [],
    "metadata": {
     "collapsed": false
    }
   }
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}